#include <stdlib.h>
#include <stdio.h>
#include <pthread.h>
#include <unistd.h>
#include <signal.h>
#include <semaphore.h>
#include <math.h>
#include <string.h>
#include <float.h>
#include <sys/time.h>
#include "bzrc.h"

#define IP_ADDRESS "127.0.0.1"
#define FLAG_RADIUS 3.0f

#define SPEED_SCALAR 1.2f

#define FLAG_STEP 0.7f // this is alpha
#define OBSTACLE_STEP 0.65f // this is beta
#define TANGENT_STEP 0.001f // beta, but for tangent field

#define FLAG_SPREAD 40.0f
#define OBSTACLE_SPREAD 60.0f
#define TANGENT_SPREAD 0.1f

// Numerical Constants
#define PI 3.14159265358979323846
#define F_INFINITY 50

#define WORLD_UPDATE_RATE 100000
#define AGENT_UPDATE_RATE 100000

#define BLUE_BASE 0
#define PURPLE_BASE 1
#define GREEN_BASE 2
#define RED_BASE 3

extern int debug;
int running;
sem_t mutex;

typedef struct {
	FILE* fSocket;
	flag* flags;
	int flagCount;
	tank* tanks;
	int tankCount;
	base* bases;
	int baseCount;
        obstacle* obstacles;
        int obstacleCount;
} world_info;

typedef struct {
	FILE* fSocket;
	int tankID;
	world_info* world;
        int flagGoalID;
} param;


// Thread functions
void *driver(void* args);
void *updater(void* args);
void *gunner(void* args);

// Potential field functions
pair seekGoal(float x, float y, float xg, float yg, float radiusg, float* retSpeed);
pair avoidObstacle(float x, float y, float xo, float yo, float radiuso);
pair circleObstacle(float x, float y, float xo, float yo, float radiusObs);
float move(FILE* fSocket, int tankID, float angle, float previousAngle, pair delta, float speed, float timeDiff);

// Helper functions
void intHandler();
float getDistanceBetween(float x, float y, float xg, float yg);
float getAngleBetween(float x, float y, float xg, float yg);
float normalizeAngle(float angle);
int timeval_subtract(struct timeval *result, struct timeval *t2, struct timeval *t1);

int main(int argc, char* argv[]) {
	int tcpSocket = 0;
	FILE* fSocket = 0;
	pthread_t* threads;
	pthread_t worldThread;
	param* threadArgs;
	world_info world;
	int threadCount;
	int i;
	running = 1;
	signal(SIGINT, intHandler);
	signal(SIGKILL, intHandler);
	sem_init(&mutex, 0, 0);
	if (argc < 2) {
		fprintf(stderr, "usage: pf_agent numagents port [debug 1:0]\n");
		exit(-1);
	}
	if (argc == 4 && atoi(argv[3]) == 1) {
		debug = 1;
	}
	threadCount = atoi(argv[1]);
	threads = (pthread_t*)malloc(sizeof(pthread_t) * threadCount);
	threadArgs = (param*)malloc(sizeof(param) * threadCount);
	BZRCConnect(IP_ADDRESS, ((argc == 2) ? 50000 : atoi(argv[2])), &tcpSocket, &fSocket);
	world.fSocket = fSocket;
	pthread_create(&worldThread, NULL, updater, (void*)&world);
	sem_wait(&mutex); // Wait for world data to be populated
	for (i = 0; i < threadCount; i++) {
		threadArgs[i].fSocket = fSocket;
		threadArgs[i].tankID = i;
		threadArgs[i].world = &world;
		threadArgs[i].flagGoalID = i % 3;
		pthread_create(&threads[i], NULL, driver, (void*)&threadArgs[i]);
	}
	for (i = 0; i < threadCount; i++) {
		pthread_join(threads[i], NULL);
	}
	pthread_join(worldThread, NULL);
	BZRCDisconnect(tcpSocket, fSocket);
	sem_destroy(&mutex);
	free(threadArgs);
	free(threads);
	printf("Agents shut down\n");
	return 0;
}

int nearObstacle(tank* tank, obstacle* obstacle) {
	float distToCenter = sqrt(pow(obstacle->center.x - tank->location.x, 2) +
				  pow(obstacle->center.y - tank->location.y, 2));
	if (distToCenter < obstacle->radius + OBSTACLE_SPREAD) {
		return 1;
	}
	return 0;
}

char* getTeamName(int color)
{
  if (color == 0)
    return "blue";
  else if (color == 1)
    return "purple";
  else if (color == 2)
    return "green";
  else if (color == 3)
    return "red";
  return "";
}

void *driver(void* args) {
	param* params = (param*)args;
	FILE* fSocket = params->fSocket;
	world_info* world = params->world;
	int tankID = params->tankID;
	pthread_t gunnerThread;
	pthread_create(&gunnerThread, NULL, gunner, args);
	pair destination;
	pair totalDelta;
	pair currentDelta;
	float speed;
	int i;
	float previousAngle = 0;
	int flagId = params->flagGoalID;
	char* flagIdName = getTeamName(flagId);
	struct timeval tvBegin, tvEnd, tvDiff;
	while (running) {
		gettimeofday(&tvBegin, NULL);
		usleep(AGENT_UPDATE_RATE);
		gettimeofday(&tvEnd, NULL);
		timeval_subtract(&tvDiff, &tvEnd, &tvBegin);
		//if (strcmp("red",world->flags[0].holderColor) != 0) { // [2] is green flag
				  
		if (strcmp(flagIdName, world->tanks[tankID].flag) == 0) { // [2] is green flag
			destination.x = world->bases[RED_BASE].corners[3].x+15;
			destination.y = world->bases[RED_BASE].corners[3].y-15;
		}
		else if (strcmp("red",world->flags[flagId].holderColor) == 0) {
			destination.x = world->bases[flagId].corners[3].x;
			destination.y = world->bases[flagId].corners[3].y;
		}
		else {
			destination.x = world->flags[flagId].location.x;
			destination.y = world->flags[flagId].location.y;  
		}
		currentDelta = seekGoal(
			world->tanks[tankID].location.x,
			world->tanks[tankID].location.y,
			destination.x,
			destination.y,
			FLAG_RADIUS,
			&speed
		);
		totalDelta.x = currentDelta.x;
		totalDelta.y = currentDelta.y;

		for (i = 0; i < world->obstacleCount; i++) {
			currentDelta = avoidObstacle(
				world->tanks[tankID].location.x,
				world->tanks[tankID].location.y,
				world->obstacles[i].center.x,
				world->obstacles[i].center.y,
				world->obstacles[i].radius
			);
			totalDelta.x += currentDelta.x;
			totalDelta.y += currentDelta.y;

			currentDelta = circleObstacle(
				world->tanks[tankID].location.x,
				world->tanks[tankID].location.y,
				world->obstacles[i].center.x,
				world->obstacles[i].center.y,
				world->obstacles[i].radius
			);
			totalDelta.x += currentDelta.x;
			totalDelta.y += currentDelta.y;
		}
		speed = sqrt(pow(totalDelta.x,2) + pow(totalDelta.y,2)) / 20;
		previousAngle = move(
			fSocket,
			tankID,
			world->tanks[tankID].angle,
			previousAngle,
			totalDelta,
			speed,
			(float)tvDiff.tv_usec
		);
	}
	setSpeed(fSocket, tankID, 0);
	setAngVel(fSocket, tankID, 0);
	pthread_join(gunnerThread, NULL);
	return 0;
}

void *gunner(void* args) {
	param* params = (param*)args;
	FILE* fSocket = params->fSocket;
	int tankID = params->tankID;
	int sleeptime;
	while (running) {
		//shoot(fSocket, tankID);
		sleeptime = (rand() % 1000000) + 1500000;
		usleep(sleeptime);
	}
	return 0;
}


// Some shared memory problems and memory leak problems exist here
// They'd be easy to remove if they became a problem.  I'll do it later.
// world structures need to be freed when not in use by any tank, and
// world needs to be locked by updater thread when it performs updates
void *updater(void* args) {
	FILE* fSocket;
	world_info* world = (world_info*)args;
	fSocket = world->fSocket;
	getFlags(fSocket, &world->flags, &world->flagCount);
	getMyTanks(fSocket, &world->tanks, &world->tankCount);
	getBases(fSocket, &world->bases, &world->baseCount); // bases are static, don't update
	getObstacles(fSocket, &world->obstacles, &world->obstacleCount);
	sem_post(&mutex);
	while (running) {
		usleep(WORLD_UPDATE_RATE);
		free(world->flags);
		getFlags(fSocket, &world->flags, &world->flagCount);
		free(world->tanks);
		getMyTanks(fSocket, &world->tanks, &world->tankCount);
	}
	return 0;
}

pair seekGoal(float x, float y, float xg, float yg, float radiusg, float* retSpeed) {
	float distance = getDistanceBetween(x, y, xg, yg);
	float angle = getAngleBetween(x, y, xg, yg);
	float speed = 0;
	pair delta;
	delta.x = 0;
	delta.y = 0;
	if (distance < radiusg) {
		delta.x = 0;
		delta.y = 0;
		speed = 0;
	}
	else if (radiusg <= distance && distance <= (FLAG_SPREAD + radiusg)) {
		delta.x = FLAG_STEP * (distance - radiusg) * cos(angle);
		delta.y = FLAG_STEP * (distance - radiusg) * sin(angle);
		speed = sqrt(pow(delta.x,2) + pow(delta.y,2));
	}
	else if (distance > (FLAG_SPREAD + radiusg)) {
		delta.x = FLAG_STEP * FLAG_SPREAD * cos(angle);
		delta.y = FLAG_STEP * FLAG_SPREAD * sin(angle);
		speed = 1.0;
	}
	*retSpeed = speed;
	return delta;
}

pair avoidObstacle(float x, float y, float xo, float yo, float radiusObs) {
	float distance = getDistanceBetween(x, y, xo, yo);
	float angle = getAngleBetween(x, y, xo, yo);
	pair delta;
	delta.x = 0;
	delta.y = 0;

	if (distance < radiusObs) {
		delta.x = -cos(angle) * F_INFINITY;
		delta.y = -sin(angle) * F_INFINITY;
	}
	else if (radiusObs <= distance && distance <= (OBSTACLE_SPREAD + radiusObs)) {
		delta.x = -OBSTACLE_STEP * (OBSTACLE_SPREAD + radiusObs - distance) * cos(angle);
		delta.y = -OBSTACLE_STEP * (OBSTACLE_SPREAD + radiusObs - distance) * sin(angle);
	}
	else if (distance > (OBSTACLE_SPREAD + radiusObs)) {
		delta.x = 0;
		delta.y = 0;
	}

	return delta;
}

pair circleObstacle(float x, float y, float xo, float yo, float radiusObs) {
	float distance = getDistanceBetween(x, y, xo, yo);
	float angle = getAngleBetween(x, y, xo, yo) + 90;
	pair delta;
	delta.x = 0;
	delta.y = 0;

	if (distance < radiusObs) {
		delta.x = -cos(angle) * F_INFINITY;
		delta.y = -sin(angle) * F_INFINITY;
	}
	else if (radiusObs <= distance && distance <= (TANGENT_SPREAD + radiusObs)) {
		delta.x = -TANGENT_STEP * (TANGENT_SPREAD + radiusObs - distance) * cos(angle);
		delta.y = -TANGENT_STEP * (TANGENT_SPREAD + radiusObs - distance) * sin(angle);
	}
	else if (distance > (TANGENT_SPREAD + radiusObs)) {
		delta.x = 0;
		delta.y = 0;
	}

	return delta;
}

float move(FILE* fSocket, int tankID, float currentAngle, float previousAngle, pair delta, float speed, float timeDiff) {
	float goalAngle = atan2(delta.y, delta.x);
	currentAngle = normalizeAngle(currentAngle);
	float errorAngle = goalAngle - currentAngle;
	float newAngle = errorAngle + ((errorAngle - previousAngle) / timeDiff);
	if (newAngle > PI) {
		newAngle = newAngle - 2 * PI;
	}
	else if (newAngle < -PI) {
		newAngle = 2 * PI + newAngle;
	}

	speed = SPEED_SCALAR * (-pow((fabs(newAngle / PI)), (float)1.0f/4.0f) + 1);
//printf("speed: %02.1f\n",speed);
	setSpeed(fSocket, tankID, speed);
	setAngVel(fSocket, tankID, newAngle / PI);
	return newAngle; // success
}

float normalizeAngle(float angle) {
	float normalizedAngle;
	normalizedAngle = fmod(angle,(2 * PI));
	if (normalizedAngle >= PI) {
		normalizedAngle -= (2 * PI);
	}
	else if (normalizedAngle <= -PI) {
		normalizedAngle += (2 * PI);
	}
	return normalizedAngle;
}

float getDistanceBetween(float x, float y, float xg, float yg) {
	float distance;
	distance = sqrt(pow((x - xg),2) + pow((y - yg),2));
	return distance;
}

float getAngleBetween(float x, float y, float xg, float yg) {
	float angle;
	angle = atan2((yg - y),(xg - x));
	return angle;
}

void intHandler() {
	printf("Shutting down agents...\n");
	running = 0;
	return;
}

int timeval_subtract(struct timeval *result, struct timeval *t2, struct timeval *t1) {
    long int diff = (t2->tv_usec + 1000000 * t2->tv_sec) - (t1->tv_usec + 1000000 * t1->tv_sec);
    result->tv_sec = diff / 1000000;
    result->tv_usec = diff % 1000000;

    return (diff<0);
}


